/**
 * Tests the motors in Vel2 mode where each motor is 
 * controlled separately. Attempts to determine the speed
 * for each motor to be set at independently. 
 *
 **/

#include <libplayerc++/playerc++.h>
#include <iostream>

#include "args.h"


int main(int argc, char **argv)
{
  parse_args(argc,argv);

  // we throw exceptions on creation if we fail
  try
  {
    using namespace PlayerCc;

    PlayerClient robot(gHostname, gPort);
    Position2dProxy pp(&robot, gIndex);
    LaserProxy lp(&robot, gIndex);

    std::cout << robot << std::endl;

    pp.SetMotorEnable (true);
    pp.ResetOdometry();
    
    // go into read-think-act loop
    for(;;)
    {
      double leftSpeed = 0;
      double rightSpeed = 0;
      int option;
      double constSpeed = 0.0;

      leftSpeed=0.5;
      rightSpeed=0.5;
      std::cout << "adjust with constant? (1)yes (0)no " << std::endl;
      std::cin >> option;
      if(option==1){
	leftSpeed=0.5;
	rightSpeed=0.5;
	std::cout << "input constant: ";
	std::cin >> constSpeed;
	leftSpeed=leftSpeed*constSpeed;
	rightSpeed=rightSpeed*constSpeed;
	std::cout << "left: " << leftSpeed << std::endl
		  << "right: " << rightSpeed << std::endl;
      }
      else{
	std::cout << "left: ";
	std::cin >> leftSpeed;
	std::cout << "right: ";
	std::cin >> rightSpeed;
      }

      // heading
      
      //double speed;
      //double Yspeed;
      //double yaw;
      //std::cout << "speed (m/s): ";
      //std::cin >> speed;
      //std::cout << "Y Speed: ";
      //std::cin >> Yspeed ;
      //std::cout << "yaw (degrees): ";
      //std::cin >> yaw	;
      
      //pp.SetVelHead(speed,Yspeed,yaw);
      
      pp.SetSpeed(leftSpeed,rightSpeed);
    }
  }
  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
